#include "PID.h"

//===================================================变量定义===================================================
PID_param SE_pid_x,SE_pid_y,Motor_L_pid,Motor_R_pid,Motor_pid;

//===================================================函数===================================================
/*---------------------------------------------------------------
 【函    数】(1)PID参数初始化
 【功    能】将PID参数进行初始化
 【参    数】无
 【返 回 值】无
 【注意事项】PID_init();
 ----------------------------------------------------------------*/
void PID_init(void)
{
	//舵机
	SE_pid_x.kp = 10;
	SE_pid_x.kd = 0.5;
	SE_pid_x.error = 0;
	SE_pid_x.pre_error = 0;
	SE_pid_x.pre_pre_error = 0;
	
	SE_pid_y.kp = 10;
	SE_pid_y.kd = 0.5;
	SE_pid_y.error = 0;
	SE_pid_y.pre_error = 0;
	SE_pid_y.pre_pre_error = 0;
	
	//电机
	Motor_L_pid.kp = 10;
	Motor_L_pid.kd = 0.5;
	Motor_L_pid.error = 0;
	Motor_L_pid.pre_error = 0;
	Motor_L_pid.pre_pre_error = 0;
	
	Motor_R_pid.kp = 10;
	Motor_R_pid.kd = 0.5;
	Motor_R_pid.error = 0;
	Motor_R_pid.pre_error = 0;
	Motor_R_pid.pre_pre_error = 0;
	
	Motor_pid.kp = 10;
	Motor_pid.kd = 0.5;
	Motor_pid.error = 0;
	Motor_pid.pre_error = 0;
	Motor_pid.pre_pre_error = 0;
	
}
/*---------------------------------------------------------------
 【函    数】(2)PID_posision_coord_x(int target_x)
 【功    能】舵机x轴运动位置式PID控制
 【参    数】1.目标x坐标值
 【返 回 值】无
 【注意事项】
 ----------------------------------------------------------------*/
int PID_posision_coord_x(int target_x)
{
	SE_pid_x.error = Current_x - target_x;
	int P_term = SE_pid_x.kp * SE_pid_x.error;
	int D_term = SE_pid_x.kd * (SE_pid_x.error - SE_pid_x.pre_error);
	P_term = Limit_int(P_term,10,15);  
	D_term = Limit_int(D_term,0,5);
	SE_pid_x.pre_error = SE_pid_x.error;
	
	return (P_term+D_term);
}
/*---------------------------------------------------------------
 【函    数】(3)PID_posision_coord_y(int target_y)
 【功    能】舵机y轴运动位置式PID控制
 【参    数】1.目标y坐标值
 【返 回 值】无
 【注意事项】
 ----------------------------------------------------------------*/
int PID_posision_coord_y(int target_y)
{
	SE_pid_y.error = target_y - Current_y;
	int P_term = SE_pid_y.kp * SE_pid_y.error;
	int D_term = SE_pid_y.kd * (SE_pid_y.error - SE_pid_y.pre_error);
	P_term = Limit_int(P_term,10,15);  
	D_term = Limit_int(D_term,0,5);
	SE_pid_y.pre_error = SE_pid_y.error;
	
	return (P_term+D_term);
}
/*---------------------------------------------------------------
 【函    数】(3)PID_posision_coord_y(int target_y)
 【功    能】舵机y轴运动位置式PID控制
 【参    数】1.目标y坐标值
 【返 回 值】无
 【注意事项】
 ----------------------------------------------------------------*/
int PID_posision_Motor(int target_y)
{
	SE_pid_y.error = target_y - Current_y;
	int P_term = SE_pid_y.kp * SE_pid_y.error;
	int D_term = SE_pid_y.kd * (SE_pid_y.error - SE_pid_y.pre_error);
	P_term = Limit_int(P_term,10,15);  
	D_term = Limit_int(D_term,0,5);
	SE_pid_y.pre_error = SE_pid_y.error;
	
	return (P_term+D_term);
}